#include "CvCodeBook.h"

CvCodeBook::CvCodeBook()
{
	m_numEntries = 0;
	m_time = 0;
}

CvCodeBook::~CvCodeBook()
{

}

int CvCodeBook::cvUpdateCodeBook(uchar *pixel, unsigned int *cbBounds, int numChannels)
{
	m_time++;
	unsigned int *high = new unsigned int[numChannels];
	unsigned int *low = new unsigned int[numChannels];
	int n, i;
	for(n = 0; n < numChannels; n++)
	{
		high[n] = *(pixel+n) + *(cbBounds+n);
		if(high[n] > 255)
			high[n] = 255;
		low[n] = *(pixel+n) - *(cbBounds+n);
		if(low[n] < 0)
			low[n] = 0;
	}
	int matchChannels;
	for(i = 0; i < m_numEntries; i++)
	{
		matchChannels = 0;
		for(n = 0; n < numChannels; n++)
		{
			if((m_codeWords[i]->learnLow[n] <= *(pixel+n)) && (*(pixel+n) <= m_codeWords[i]->learnHigh[n]))
			{
				matchChannels++;
			}
			else
			{
				break;
			}
		}
		if(matchChannels == numChannels)
		{
			m_codeWords[i]->tLastUpdate = m_time;
			for(n = 0; n < numChannels; n++)
			{
				if(m_codeWords[i]->max[n] < *(pixel+n))
				{
					m_codeWords[i]->max[n] = *(pixel+n);
				}
				else if(m_codeWords[i]->min[n] > *(pixel+n))
				{
					m_codeWords[i]->min[n] = *(pixel+n);
				}
			}
			break;
		}
	}
	for(int s = 0; s < m_numEntries; s++)
	{
		int negRun = m_time - m_codeWords[s]->tLastUpdate;
		if(m_codeWords[s]->stale < negRun) m_codeWords[s]->stale = negRun;
	}
	if(i == m_numEntries)
	{
		CvCodeWord **codeWords = new CvCodeWord*[m_numEntries+1];
		for(int s = 0; s < m_numEntries; s++)
		{
			codeWords[s] = m_codeWords[s];
		}
		codeWords[m_numEntries] = new CvCodeWord;
		if(m_numEntries > 0) delete[] m_codeWords;
		m_codeWords = codeWords;
		for(n = 0; n < numChannels; n++)
		{
			m_codeWords[m_numEntries]->learnHigh[n] = high[n];
			m_codeWords[m_numEntries]->learnLow[n] = low[n];
			m_codeWords[m_numEntries]->max[n] = *(pixel+n);
			m_codeWords[m_numEntries]->min[n] = *(pixel+n);
		}
		m_codeWords[m_numEntries]->tLastUpdate = m_time;
		m_codeWords[m_numEntries]->stale = 0;
		m_numEntries += 1;
	}
	for(n = 0; n < numChannels; n++)
	{
		if(m_codeWords[i]->learnHigh[n] < high[n]) 
			m_codeWords[i]->learnHigh[n] += 1;
		if(m_codeWords[i]->learnLow[n] > low[n]) 
			m_codeWords[i]->learnLow[n] -= 1;
	}
	delete[] high;
	delete[] low;
	return i;
}

int CvCodeBook::cvClearStaleEntries()
{
	int staleThresh = m_time >> 1;
	int *keep = new int[m_numEntries];
	int keepCount = 0;
	for(int i = 0; i < m_numEntries; i++)
	{
		if(m_codeWords[i]->stale > staleThresh)
		{
			keep[i] = 0;
		}
		else
		{
			keep[i] = 1;
			keepCount += 1;
		}
	}
	m_time = 0;
	CvCodeWord **codeWords = new CvCodeWord*[keepCount];
	int k = 0;
	for(int i = 0; i < m_numEntries; i++)
	{
		if(keep[i])
		{
			codeWords[k] = m_codeWords[i];
			codeWords[k]->tLastUpdate = 0;
			k++;
		}
	}
	delete[] keep;
	delete[] m_codeWords;
	m_codeWords = codeWords;
	int numCleared = m_numEntries - keepCount;
	m_numEntries = keepCount;
	return numCleared;
}

uchar CvCodeBook::cvBackgroundDiff(uchar *pixel, int *minMod, int *maxMod, int numChannels)
{
	int matchChannels;
	int i;
	for(i = 0; i < m_numEntries; i++)
	{
		matchChannels = 0;
		for(int n = 0; n < numChannels; n++)
		{
			if((m_codeWords[i]->min[n]-minMod[n] <= *(pixel+n)) && (*(pixel+n) <= m_codeWords[i]->max[n]+maxMod[n]))
			{
				matchChannels++;
			}
			else
			{
				break;
			}
		}
		if(matchChannels == numChannels)
		{
			break;
		}
	}
	if(i >= m_numEntries)
		return 255;
	return 0;
}